<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN">

<!-- 
	Copyright (C) 2007, 2008, 2009, 2010, 2011. PARP Research Group.
	<http://perception.inf.um.es>
	University of Murcia, Spain.

	This file is part of the QVision library.

	QVision is free software: you can redistribute it and/or modify
	it under the terms of the GNU Lesser General Public License as
	published by the Free Software Foundation, version 3 of the License.

	QVision is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public
	License along with QVision. If not, see <http://www.gnu.org/licenses/>.
-->

<html><head><meta http-equiv="content-Type" content="text/html;charset=UTF-8">
<title>QVision: Qt&#39;s Image, Video and Computer Vision Library</title>
<meta name="title" content="QVision" />
<meta name="dc.title" content="QVision" />
<meta name="url" content="http://perception.inf.um.es/QVision" />
<meta name="author" content="PARP Research Group - http://perception.inf.um.es" />
<meta name="revisit-after" content="30 DAYS"/>
<meta name="robots" content="index,follow"/>
<meta name="classification" content="*">
<meta name="rating" content="Safe For Kids">
<meta name="distribution" content="GLOBAL"/>
<meta name="description" content="Qt's Image, Video and Computer Vision Library"/>
<meta name="page-topic" content="Computer Vision research and prototype programming"/>
<meta name="geo.country" content="ES" />

<!--
Keywords:
By license:		open source, gnu, lgpl, gpl, free
By theme:		computer vision, image processing, robotics, programming, source, development
By usage:		library, toolkit, framework, prototype, application
By programming specs:	object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping
Interoperability with:	Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack
Functionallity:		image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface
Main data-types:	matrix, vector, tensor, quaternion, image, polyline
Video sources:		webcam, camera, stream
Devices:		embedded, desktop computer, laptop, mini-laptop
Authors:		PARP research group. University of Murcia, Spain.
-->

<meta name="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="keywords" content="augmented reality, sfm, structure from motion, open source, gnu, lgpl, gpl, free, computer vision, image processing, robotics, programming, source, development, library, toolkit, framework, prototype, application, object oriented, c++, block programming, reusability, gui, graphical, parallel computing, high performance, GPU, prototyping, Qt, GSL, GNU Scientific library, OpenCV, CGAL, QWT, CUDA, mplayer, IPP, Intel Image Performance Primitives, blas, lapack, image features, matrix algebra, projective geometry, mser, function minimization, function optimization, canny operator, harris operator, corner detection, performance evaluation, cpu usage, graphical interface, matrix, vector, tensor, quaternion, image, polyline, webcam, camera, stream, embedded, desktop computer, laptop, mini-laptop, University of Murcia, Spain, PARP research group, vision por computador"/>
<meta http-equiv="pragma" content="no-cache"/>
<meta http-equiv="title" content="QVision"/>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
<link href="tabs.css" rel="stylesheet" type="text/css" />
<link rel="shortcut icon" href="favicon.ico" />
</head><body>

<table width="100%"><tr>
	<td><a href="http://perception.inf.um.es/"><img src="parp.png" border="0" /> <big>PARP Research Group</big></a></td>
	<td align="right"><a href="http://www.um.es/"><big>Universidad de Murcia</big> <img src="um.png" border="0" /></a></td>
</tr></table>

<hr /><br />

<table width="95%" align="center"><tr><td>

<!-- Generated by Doxygen 1.6.3 -->
<script type="text/javascript"><!--
var searchBox = new SearchBox("searchBox", "search",false,'Search');
--></script>
<div class="navigation" id="top">
  <div class="tabs">
    <ul>
      <li><a href="index.html"><span>Main&nbsp;Page</span></a></li>
      <li><a href="pages.html"><span>Related&nbsp;Pages</span></a></li>
      <li><a href="modules.html"><span>Modules</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
          <form id="FSearchBox" action="search.php" method="get">
            <img id="MSearchSelect" src="search/search.png" alt=""/>
            <input type="text" id="MSearchField" name="query" value="Search" size="20" accesskey="S" 
                   onfocus="searchBox.OnSearchFieldFocus(true)" 
                   onblur="searchBox.OnSearchFieldFocus(false)"/>
          </form>
        </div>
      </li>
    </ul>
  </div>
  <div class="tabs">
    <ul>
      <li><a href="files.html"><span>File&nbsp;List</span></a></li>
      <li><a href="globals.html"><span>File&nbsp;Members</span></a></li>
    </ul>
  </div>
<h1>src/qvsfm/laSBA/laSBAWrapper.cpp</h1><a href="laSBAWrapper_8cpp.html">Go to the documentation of this file.</a><div class="fragment"><pre class="fragment"><a name="l00001"></a>00001 <span class="comment">/*</span>
<a name="l00002"></a>00002 <span class="comment"> *      Copyright (C) 2011, 2012. PARP Research Group.</span>
<a name="l00003"></a>00003 <span class="comment"> *      &lt;http://perception.inf.um.es&gt;</span>
<a name="l00004"></a>00004 <span class="comment"> *      University of Murcia, Spain.</span>
<a name="l00005"></a>00005 <span class="comment"> *</span>
<a name="l00006"></a>00006 <span class="comment"> *      This file is part of the QVision library.</span>
<a name="l00007"></a>00007 <span class="comment"> *</span>
<a name="l00008"></a>00008 <span class="comment"> *      QVision is free software: you can redistribute it and/or modify</span>
<a name="l00009"></a>00009 <span class="comment"> *      it under the terms of the GNU Lesser General Public License as</span>
<a name="l00010"></a>00010 <span class="comment"> *      published by the Free Software Foundation, version 3 of the License.</span>
<a name="l00011"></a>00011 <span class="comment"> *</span>
<a name="l00012"></a>00012 <span class="comment"> *      QVision is distributed in the hope that it will be useful,</span>
<a name="l00013"></a>00013 <span class="comment"> *      but WITHOUT ANY WARRANTY; without even the implied warranty of</span>
<a name="l00014"></a>00014 <span class="comment"> *      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the</span>
<a name="l00015"></a>00015 <span class="comment"> *      GNU Lesser General Public License for more details.</span>
<a name="l00016"></a>00016 <span class="comment"> *</span>
<a name="l00017"></a>00017 <span class="comment"> *      You should have received a copy of the GNU Lesser General Public</span>
<a name="l00018"></a>00018 <span class="comment"> *      License along with QVision. If not, see &lt;http://www.gnu.org/licenses/&gt;.</span>
<a name="l00019"></a>00019 <span class="comment"> */</span>
<a name="l00020"></a>00020 
<a name="l00024"></a>00024 
<a name="l00025"></a>00025 <span class="preprocessor">#define SBA_OPTSSZ        5</span>
<a name="l00026"></a>00026 <span class="preprocessor"></span><span class="preprocessor">#define SBA_INFOSZ        10</span>
<a name="l00027"></a>00027 <span class="preprocessor"></span><span class="preprocessor">#define SBA_ERROR         -1</span>
<a name="l00028"></a>00028 <span class="preprocessor"></span><span class="preprocessor">#define SBA_INIT_MU       1E-03</span>
<a name="l00029"></a>00029 <span class="preprocessor"></span><span class="preprocessor">#define SBA_STOP_THRESH   1E-12</span>
<a name="l00030"></a>00030 <span class="preprocessor"></span><span class="preprocessor">#define SBA_CG_NOPREC     0</span>
<a name="l00031"></a>00031 <span class="preprocessor"></span><span class="preprocessor">#define SBA_CG_JACOBI     1</span>
<a name="l00032"></a>00032 <span class="preprocessor"></span><span class="preprocessor">#define SBA_CG_SSOR       2</span>
<a name="l00033"></a>00033 <span class="preprocessor"></span><span class="preprocessor">#define SBA_VERSION       &quot;1.6 (Aug. 2009)&quot;</span>
<a name="l00034"></a>00034 <span class="preprocessor"></span>
<a name="l00035"></a>00035 <span class="preprocessor">#include &lt;QTime&gt;</span>
<a name="l00036"></a>00036 <span class="preprocessor">#include &lt;<a class="code" href="laSBAWrapper_8h.html" title="File from the QVision library.">qvsfm/laSBA/laSBAWrapper.h</a>&gt;</span>
<a name="l00037"></a>00037 <span class="preprocessor">#include &lt;qvsfm/laSBA/sba-1.6/compiler.h&gt;</span>
<a name="l00038"></a>00038 <span class="preprocessor">#include &lt;qvsfm/laSBA/sba-1.6/sba.h&gt;</span>
<a name="l00039"></a>00039 
<a name="l00040"></a>00040 <span class="keyword">const</span> <span class="keywordtype">int</span>       CAMERA_VECTOR_SIZE=7,   <span class="comment">// Camera size: 4 rot params + 3 trans params.</span>
<a name="l00041"></a>00041                 POINT3D_SIZE=3,                 <span class="comment">// 3D Point size.</span>
<a name="l00042"></a>00042                 POINT2D_SIZE=2;                 <span class="comment">// 2D Points size.</span>
<a name="l00043"></a>00043 
<a name="l00044"></a>00044 <span class="keywordtype">void</span> initData(  <span class="keyword">const</span> QList&lt;QVEuclideanMapping3&gt; &amp;cameras,
<a name="l00045"></a>00045                 <span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D,
<a name="l00046"></a>00046                 <span class="keyword">const</span> QList&lt; QHash&lt;int, QPointF&gt; &gt; &amp;pointProjections,
<a name="l00047"></a>00047                 <span class="keywordtype">double</span>  *motstruct,
<a name="l00048"></a>00048                 <span class="keywordtype">double</span> *imgpts,
<a name="l00049"></a>00049                 <span class="keywordtype">char</span> *vmask
<a name="l00050"></a>00050                 );
<a name="l00051"></a>00051 
<a name="l00052"></a>00052 <span class="keywordtype">void</span> img_proj2(<span class="keywordtype">int</span> numCam, <span class="keywordtype">int</span> numPoint, <span class="keywordtype">double</span> *cam, <span class="keywordtype">double</span> *p3D, <span class="keywordtype">double</span> *p2D, <span class="keywordtype">void</span> *);
<a name="l00053"></a>00053 
<a name="l00054"></a>00054 <span class="keywordtype">void</span> readData(  <span class="keyword">const</span> <span class="keywordtype">double</span>    *motstruct,
<a name="l00055"></a>00055                 <span class="keyword">const</span> <span class="keywordtype">int</span>       numFrames,      <span class="comment">// Frames</span>
<a name="l00056"></a>00056                 <span class="keyword">const</span> <span class="keywordtype">int</span>       numPoints3D,    <span class="comment">// Number of points</span>
<a name="l00057"></a>00057                 QList&lt;QVEuclideanMapping3&gt; &amp;refinedCameras,
<a name="l00058"></a>00058                 QList&lt;QV3DPointF&gt; &amp;refinedPoints3D);
<a name="l00059"></a>00059 
<a name="l00060"></a>00060 <span class="keywordtype">double</span> final_reprojection_error, initial_reprojection_error;
<a name="l00061"></a>00061 <span class="keywordtype">int</span> sba_elapsed_milisecs = 0, sba_iterations = 0, sba_stop_condition = -1;
<a name="l00062"></a>00062 
<a name="l00063"></a>00063 <span class="comment">/* Euclidean bundle adjustment demo using the sba package */</span>
<a name="l00064"></a>00064 <span class="comment">// -------------------------------------------------------</span>
<a name="l00065"></a>00065 <span class="keywordtype">void</span> my_img_proj(<span class="keywordtype">int</span> numCam, <span class="keywordtype">int</span> numPoint, <span class="keywordtype">double</span> *camera, <span class="keywordtype">double</span> *p3D, <span class="keywordtype">double</span> *p2D, <span class="keywordtype">void</span> *)
<a name="l00066"></a>00066         {
<a name="l00067"></a>00067         <span class="keyword">const</span> <span class="keywordtype">double</span>    camera0 = camera[0],
<a name="l00068"></a>00068                                 camera1 = camera[1],
<a name="l00069"></a>00069                                 camera2 = camera[2],
<a name="l00070"></a>00070                                 camera3 = camera[3],
<a name="l00071"></a>00071                                 camera4 = camera[4],
<a name="l00072"></a>00072                                 camera5 = camera[5],
<a name="l00073"></a>00073                                 camera6 = camera[6];
<a name="l00074"></a>00074         <span class="keyword">const</span> <span class="keywordtype">double</span>    p3D0 = p3D[0],
<a name="l00075"></a>00075                                 p3D1 = p3D[1],
<a name="l00076"></a>00076                                 p3D2 = p3D[2];
<a name="l00077"></a>00077 
<a name="l00078"></a>00078         <span class="comment">/*</span>
<a name="l00079"></a>00079 <span class="comment">        Maple code to evaluate the reprojection error:</span>
<a name="l00080"></a>00080 <span class="comment">        --------------------------------</span>
<a name="l00081"></a>00081 <span class="comment">        with(VectorCalculus)</span>
<a name="l00082"></a>00082 <span class="comment">        with(LinearAlgebra):</span>
<a name="l00083"></a>00083 <span class="comment">        Norm2 := v -&gt; sqrt(v(1)^2 + v(2)^2 + v(3)^2):</span>
<a name="l00084"></a>00084 <span class="comment">        FromHomogeneous2D := p3D -&gt; Vector([p3D(1)/p3D(3), p3D(2)/p3D(3)]):</span>
<a name="l00085"></a>00085 <span class="comment">        CrossProductMatrix := v -&gt; Matrix([[0, -v(3), v(2)], [v(3), 0, -v(1)], [-v(2), v(1), 0]]):</span>
<a name="l00086"></a>00086 <span class="comment">        QuaternionToRotationMatrix := q -&gt;</span>
<a name="l00087"></a>00087 <span class="comment">                Matrix([[1,0,0],[0,1,0],[0,0,1]]) +</span>
<a name="l00088"></a>00088 <span class="comment">                (2.0 / (q(1)^2+q(2)^2+q(3)^2+q(4)^2)) *</span>
<a name="l00089"></a>00089 <span class="comment">                Matrix( [       [-q(2)*q(2)-q(3)*q(3), q(1)*q(2)-q(3)*q(4), q(3)*q(1)+q(2)*q(4)],</span>
<a name="l00090"></a>00090 <span class="comment">                                [q(1)*q(2)+q(3)*q(4), -q(3)*q(3)-q(1)*q(1), q(2)*q(3)-q(1)*q(4)],</span>
<a name="l00091"></a>00091 <span class="comment">                                [q(3)*q(1)-q(2)*q(4), q(2)*q(3)+q(1)*q(4), -q(2)*q(2)-q(1)*q(1)]</span>
<a name="l00092"></a>00092 <span class="comment">                        ]):</span>
<a name="l00093"></a>00093 <span class="comment">        R := QuaternionToRotationMatrix(Vector([camera0, camera1, camera2, camera3])):</span>
<a name="l00094"></a>00094 <span class="comment">        t := Vector([camera4, camera5, camera6]):</span>
<a name="l00095"></a>00095 <span class="comment">        point3D := Vector([p3D0, p3D1, p3D2]):</span>
<a name="l00096"></a>00096 <span class="comment">        p2D := FromHomogeneous2D(R.point3D + t):</span>
<a name="l00097"></a>00097 <span class="comment">        CodeGeneration[C](p2D, optimize = true);</span>
<a name="l00098"></a>00098 <span class="comment">        */</span>
<a name="l00099"></a>00099         <span class="keyword">const</span> <span class="keywordtype">double</span> t1 = camera0 * camera0;
<a name="l00100"></a>00100         <span class="keyword">const</span> <span class="keywordtype">double</span> t2 = camera1 * camera1;
<a name="l00101"></a>00101         <span class="keyword">const</span> <span class="keywordtype">double</span> t3 = camera2 * camera2;
<a name="l00102"></a>00102         <span class="keyword">const</span> <span class="keywordtype">double</span> t4 = camera3 * camera3;
<a name="l00103"></a>00103         <span class="keyword">const</span> <span class="keywordtype">double</span> t6 = 0.1e1 / (t1 + t2 + t3 + t4);
<a name="l00104"></a>00104         <span class="keyword">const</span> <span class="keywordtype">double</span> t7 = 0.20e1 * t2;
<a name="l00105"></a>00105         <span class="keyword">const</span> <span class="keywordtype">double</span> t8 = 0.20e1 * t3;
<a name="l00106"></a>00106         <span class="keyword">const</span> <span class="keywordtype">double</span> t14 = 0.20e1 * camera0 * camera1;
<a name="l00107"></a>00107         <span class="keyword">const</span> <span class="keywordtype">double</span> t16 = 0.20e1 * camera2 * camera3;
<a name="l00108"></a>00108         <span class="keyword">const</span> <span class="keywordtype">double</span> t21 = 0.20e1 * camera2 * camera0;
<a name="l00109"></a>00109         <span class="keyword">const</span> <span class="keywordtype">double</span> t23 = 0.20e1 * camera1 * camera3;
<a name="l00110"></a>00110         <span class="keyword">const</span> <span class="keywordtype">double</span> t32 = 0.20e1 * camera1 * camera2;
<a name="l00111"></a>00111         <span class="keyword">const</span> <span class="keywordtype">double</span> t34 = 0.20e1 * camera0 * camera3;
<a name="l00112"></a>00112         <span class="keyword">const</span> <span class="keywordtype">double</span> t38 = 0.20e1 * t1;
<a name="l00113"></a>00113         <span class="keyword">const</span> <span class="keywordtype">double</span> t44 = 0.1e1 / (t6 * (t21 - t23) * p3D0 + t6 * (t32 + t34) * p3D1 + (0.1e1 + t6 * (-t7 - t38)) * p3D2 + camera6);
<a name="l00114"></a>00114         p2D[0] = ((0.1e1 + t6 * (-t7 - t8)) * p3D0 + t6 * (t14 - t16) * p3D1 + t6 * (t21 + t23) * p3D2 + camera4) * t44;
<a name="l00115"></a>00115         p2D[1] = (t6 * (t14 + t16) * p3D0 + (0.1e1 + t6 * (-t8 - t38)) * p3D1 + t6 * (t32 - t34) * p3D2 + camera5) * t44;
<a name="l00116"></a>00116         }
<a name="l00117"></a>00117 
<a name="l00118"></a>00118 <span class="keywordtype">void</span> my_img_proj_jac(<span class="keywordtype">int</span> numCam, <span class="keywordtype">int</span> numPoint, <span class="keywordtype">double</span> *camera, <span class="keywordtype">double</span> *p3D, <span class="keywordtype">double</span> * jacmRT, <span class="keywordtype">double</span> * jacmS, <span class="keywordtype">void</span> *)
<a name="l00119"></a>00119         {
<a name="l00120"></a>00120         <span class="keyword">const</span> <span class="keywordtype">double</span>    camera0 = camera[0],
<a name="l00121"></a>00121                                 camera1 = camera[1],
<a name="l00122"></a>00122                                 camera2 = camera[2],
<a name="l00123"></a>00123                                 camera3 = camera[3],
<a name="l00124"></a>00124                                 camera4 = camera[4],
<a name="l00125"></a>00125                                 camera5 = camera[5],
<a name="l00126"></a>00126                                 camera6 = camera[6];
<a name="l00127"></a>00127         <span class="keyword">const</span> <span class="keywordtype">double</span>    p3D0 = p3D[0],
<a name="l00128"></a>00128                                 p3D1 = p3D[1],
<a name="l00129"></a>00129                                 p3D2 = p3D[2];
<a name="l00130"></a>00130                 
<a name="l00131"></a>00131         <span class="comment">/*</span>
<a name="l00132"></a>00132 <span class="comment">        Maple code:</span>
<a name="l00133"></a>00133 <span class="comment">        ------------------</span>
<a name="l00134"></a>00134 <span class="comment">        J := Vector([</span>
<a name="l00135"></a>00135 <span class="comment">                diff(p2D(1), camera0), diff(p2D(1), camera1), diff(p2D(1), camera2), diff(p2D(1), camera3), diff(p2D(1), camera4), diff(p2D(1), camera5), diff(p2D(1), camera6), diff(p2D(1), p3D0), diff(p2D(1), p3D1), diff(p2D(1), p3D2),</span>
<a name="l00136"></a>00136 <span class="comment">                diff(p2D(2), camera0), diff(p2D(2), camera1), diff(p2D(2), camera2), diff(p2D(2), camera3), diff(p2D(2), camera4), diff(p2D(2), camera5), diff(p2D(2), camera6), diff(p2D(2), p3D0), diff(p2D(2), p3D1), diff(p2D(2), p3D2)</span>
<a name="l00137"></a>00137 <span class="comment">                ]):</span>
<a name="l00138"></a>00138 <span class="comment">        CodeGeneration[C](J, optimize = true);</span>
<a name="l00139"></a>00139 <span class="comment">        */</span>
<a name="l00140"></a>00140         <span class="keyword">const</span> <span class="keywordtype">double</span> t1 = camera0 * camera0;
<a name="l00141"></a>00141         <span class="keyword">const</span> <span class="keywordtype">double</span> t2 = camera1 * camera1;
<a name="l00142"></a>00142         <span class="keyword">const</span> <span class="keywordtype">double</span> t3 = camera2 * camera2;
<a name="l00143"></a>00143         <span class="keyword">const</span> <span class="keywordtype">double</span> t4 = camera3 * camera3;
<a name="l00144"></a>00144         <span class="keyword">const</span> <span class="keywordtype">double</span> t5 = t1 + t2 + t3 + t4;
<a name="l00145"></a>00145         <span class="keyword">const</span> <span class="keywordtype">double</span> t6 = t5 * t5;
<a name="l00146"></a>00146         <span class="keyword">const</span> <span class="keywordtype">double</span> t7 = 0.1e1 / t6;
<a name="l00147"></a>00147         <span class="keyword">const</span> <span class="keywordtype">double</span> t8 = 0.20e1 * t2;
<a name="l00148"></a>00148         <span class="keyword">const</span> <span class="keywordtype">double</span> t9 = 0.20e1 * t3;
<a name="l00149"></a>00149         <span class="keyword">const</span> <span class="keywordtype">double</span> t10 = -t8 - t9;
<a name="l00150"></a>00150         <span class="keyword">const</span> <span class="keywordtype">double</span> t11 = t7 * t10;
<a name="l00151"></a>00151         <span class="keyword">const</span> <span class="keywordtype">double</span> t12 = camera0 * p3D0;
<a name="l00152"></a>00152         <span class="keyword">const</span> <span class="keywordtype">double</span> t16 = 0.20e1 * camera0 * camera1;
<a name="l00153"></a>00153         <span class="keyword">const</span> <span class="keywordtype">double</span> t18 = 0.20e1 * camera2 * camera3;
<a name="l00154"></a>00154         <span class="keyword">const</span> <span class="keywordtype">double</span> t19 = t16 - t18;
<a name="l00155"></a>00155         <span class="keyword">const</span> <span class="keywordtype">double</span> t20 = t7 * t19;
<a name="l00156"></a>00156         <span class="keyword">const</span> <span class="keywordtype">double</span> t21 = p3D1 * camera0;
<a name="l00157"></a>00157         <span class="keyword">const</span> <span class="keywordtype">double</span> t24 = 0.1e1 / t5;
<a name="l00158"></a>00158         <span class="keyword">const</span> <span class="keywordtype">double</span> t25 = t24 * camera1;
<a name="l00159"></a>00159         <span class="keyword">const</span> <span class="keywordtype">double</span> t27 = 0.20e1 * t25 * p3D1;
<a name="l00160"></a>00160         <span class="keyword">const</span> <span class="keywordtype">double</span> t29 = 0.20e1 * camera2 * camera0;
<a name="l00161"></a>00161         <span class="keyword">const</span> <span class="keywordtype">double</span> t31 = 0.20e1 * camera1 * camera3;
<a name="l00162"></a>00162         <span class="keyword">const</span> <span class="keywordtype">double</span> t32 = t29 + t31;
<a name="l00163"></a>00163         <span class="keyword">const</span> <span class="keywordtype">double</span> t33 = t7 * t32;
<a name="l00164"></a>00164         <span class="keyword">const</span> <span class="keywordtype">double</span> t34 = p3D2 * camera0;
<a name="l00165"></a>00165         <span class="keyword">const</span> <span class="keywordtype">double</span> t37 = t24 * camera2;
<a name="l00166"></a>00166         <span class="keyword">const</span> <span class="keywordtype">double</span> t39 = 0.20e1 * t37 * p3D2;
<a name="l00167"></a>00167         <span class="keyword">const</span> <span class="keywordtype">double</span> t41 = t29 - t31;
<a name="l00168"></a>00168         <span class="keyword">const</span> <span class="keywordtype">double</span> t42 = t24 * t41;
<a name="l00169"></a>00169         <span class="keyword">const</span> <span class="keywordtype">double</span> t45 = 0.20e1 * camera1 * camera2;
<a name="l00170"></a>00170         <span class="keyword">const</span> <span class="keywordtype">double</span> t47 = 0.20e1 * camera0 * camera3;
<a name="l00171"></a>00171         <span class="keyword">const</span> <span class="keywordtype">double</span> t48 = t45 + t47;
<a name="l00172"></a>00172         <span class="keyword">const</span> <span class="keywordtype">double</span> t49 = t24 * t48;
<a name="l00173"></a>00173         <span class="keyword">const</span> <span class="keywordtype">double</span> t51 = 0.20e1 * t1;
<a name="l00174"></a>00174         <span class="keyword">const</span> <span class="keywordtype">double</span> t52 = -t8 - t51;
<a name="l00175"></a>00175         <span class="keyword">const</span> <span class="keywordtype">double</span> t54 = 0.1e1 + t24 * t52;
<a name="l00176"></a>00176         <span class="keyword">const</span> <span class="keywordtype">double</span> t56 = t42 * p3D0 + t49 * p3D1 + t54 * p3D2 + camera6;
<a name="l00177"></a>00177         <span class="keyword">const</span> <span class="keywordtype">double</span> t57 = 0.1e1 / t56;
<a name="l00178"></a>00178         <span class="keyword">const</span> <span class="keywordtype">double</span> t60 = 0.1e1 + t24 * t10;
<a name="l00179"></a>00179         <span class="keyword">const</span> <span class="keywordtype">double</span> t62 = t24 * t19;
<a name="l00180"></a>00180         <span class="keyword">const</span> <span class="keywordtype">double</span> t64 = t24 * t32;
<a name="l00181"></a>00181         <span class="keyword">const</span> <span class="keywordtype">double</span> t67 = t56 * t56;
<a name="l00182"></a>00182         <span class="keyword">const</span> <span class="keywordtype">double</span> t68 = 0.1e1 / t67;
<a name="l00183"></a>00183         <span class="keyword">const</span> <span class="keywordtype">double</span> t69 = (t60 * p3D0 + t62 * p3D1 + t64 * p3D2 + camera4) * t68;
<a name="l00184"></a>00184         <span class="keyword">const</span> <span class="keywordtype">double</span> t70 = t7 * t41;
<a name="l00185"></a>00185         <span class="keyword">const</span> <span class="keywordtype">double</span> t74 = 0.20e1 * t37 * p3D0;
<a name="l00186"></a>00186         <span class="keyword">const</span> <span class="keywordtype">double</span> t75 = t7 * t48;
<a name="l00187"></a>00187         <span class="keyword">const</span> <span class="keywordtype">double</span> t78 = t24 * camera3;
<a name="l00188"></a>00188         <span class="keyword">const</span> <span class="keywordtype">double</span> t80 = 0.20e1 * t78 * p3D1;
<a name="l00189"></a>00189         <span class="keyword">const</span> <span class="keywordtype">double</span> t81 = t7 * t52;
<a name="l00190"></a>00190         <span class="keyword">const</span> <span class="keywordtype">double</span> t84 = t24 * camera0;
<a name="l00191"></a>00191         <span class="keyword">const</span> <span class="keywordtype">double</span> t85 = 0.40e1 * t84;
<a name="l00192"></a>00192         <span class="keyword">const</span> <span class="keywordtype">double</span> t88 = -0.2e1 * t70 * t12 + t74 - 0.2e1 * t75 * t21 + t80 + (-0.2e1 * t81 * camera0 - t85) * p3D2;
<a name="l00193"></a>00193         <span class="keyword">const</span> <span class="keywordtype">double</span> t93 = 0.40e1 * t25;
<a name="l00194"></a>00194         <span class="keyword">const</span> <span class="keywordtype">double</span> t96 = p3D1 * camera1;
<a name="l00195"></a>00195         <span class="keyword">const</span> <span class="keywordtype">double</span> t100 = 0.20e1 * t84 * p3D1;
<a name="l00196"></a>00196         <span class="keyword">const</span> <span class="keywordtype">double</span> t101 = p3D2 * camera1;
<a name="l00197"></a>00197         <span class="keyword">const</span> <span class="keywordtype">double</span> t105 = 0.20e1 * t78 * p3D2;
<a name="l00198"></a>00198         <span class="keyword">const</span> <span class="keywordtype">double</span> t108 = p3D0 * camera1;
<a name="l00199"></a>00199         <span class="keyword">const</span> <span class="keywordtype">double</span> t112 = 0.20e1 * t78 * p3D0;
<a name="l00200"></a>00200         <span class="keyword">const</span> <span class="keywordtype">double</span> t116 = 0.20e1 * t37 * p3D1;
<a name="l00201"></a>00201         <span class="keyword">const</span> <span class="keywordtype">double</span> t121 = -0.2e1 * t70 * t108 - t112 - 0.2e1 * t75 * t96 + t116 + (-0.2e1 * t81 * camera1 - t93) * p3D2;
<a name="l00202"></a>00202         <span class="keyword">const</span> <span class="keywordtype">double</span> t126 = 0.40e1 * t37;
<a name="l00203"></a>00203         <span class="keyword">const</span> <span class="keywordtype">double</span> t129 = p3D1 * camera2;
<a name="l00204"></a>00204         <span class="keyword">const</span> <span class="keywordtype">double</span> t132 = p3D2 * camera2;
<a name="l00205"></a>00205         <span class="keyword">const</span> <span class="keywordtype">double</span> t136 = 0.20e1 * t84 * p3D2;
<a name="l00206"></a>00206         <span class="keyword">const</span> <span class="keywordtype">double</span> t139 = p3D0 * camera2;
<a name="l00207"></a>00207         <span class="keyword">const</span> <span class="keywordtype">double</span> t143 = 0.20e1 * t84 * p3D0;
<a name="l00208"></a>00208         <span class="keyword">const</span> <span class="keywordtype">double</span> t148 = -0.2e1 * t70 * t139 + t143 - 0.2e1 * t75 * t129 + t27 - 0.2e1 * t81 * t132;
<a name="l00209"></a>00209         <span class="keyword">const</span> <span class="keywordtype">double</span> t151 = camera3 * p3D0;
<a name="l00210"></a>00210         <span class="keyword">const</span> <span class="keywordtype">double</span> t154 = p3D1 * camera3;
<a name="l00211"></a>00211         <span class="keyword">const</span> <span class="keywordtype">double</span> t157 = p3D2 * camera3;
<a name="l00212"></a>00212         <span class="keyword">const</span> <span class="keywordtype">double</span> t161 = 0.20e1 * t25 * p3D2;
<a name="l00213"></a>00213         <span class="keyword">const</span> <span class="keywordtype">double</span> t167 = 0.20e1 * t25 * p3D0;
<a name="l00214"></a>00214         <span class="keyword">const</span> <span class="keywordtype">double</span> t172 = -0.2e1 * t70 * t151 - t167 - 0.2e1 * t75 * t154 + t100 - 0.2e1 * t81 * t157;
<a name="l00215"></a>00215         <span class="keyword">const</span> <span class="keywordtype">double</span> t184 = t16 + t18;
<a name="l00216"></a>00216         <span class="keyword">const</span> <span class="keywordtype">double</span> t185 = t7 * t184;
<a name="l00217"></a>00217         <span class="keyword">const</span> <span class="keywordtype">double</span> t188 = -t9 - t51;
<a name="l00218"></a>00218         <span class="keyword">const</span> <span class="keywordtype">double</span> t189 = t7 * t188;
<a name="l00219"></a>00219         <span class="keyword">const</span> <span class="keywordtype">double</span> t194 = t45 - t47;
<a name="l00220"></a>00220         <span class="keyword">const</span> <span class="keywordtype">double</span> t195 = t7 * t194;
<a name="l00221"></a>00221         <span class="keyword">const</span> <span class="keywordtype">double</span> t200 = t24 * t184;
<a name="l00222"></a>00222         <span class="keyword">const</span> <span class="keywordtype">double</span> t203 = 0.1e1 + t24 * t188;
<a name="l00223"></a>00223         <span class="keyword">const</span> <span class="keywordtype">double</span> t205 = t24 * t194;
<a name="l00224"></a>00224         <span class="keyword">const</span> <span class="keywordtype">double</span> t208 = (t200 * p3D0 + t203 * p3D1 + t205 * p3D2 + camera5) * t68;
<a name="l00225"></a>00225         jacmRT[0] = (-0.2e1 * t11 * t12 - 0.2e1 * t20 * t21 + t27 - 0.2e1 * t33 * t34 + t39) * t57 - t69 * t88;
<a name="l00226"></a>00226         jacmRT[1] = ((-0.2e1 * t11 * camera1 - t93) * p3D0 - 0.2e1 * t20 * t96 + t100 - 0.2e1 * t33 * t101 + t105) * t57 - t69 * t121;
<a name="l00227"></a>00227         jacmRT[2] = ((-0.2e1 * t11 * camera2 - t126) * p3D0 - 0.2e1 * t20 * t129 - t80 - 0.2e1 * t33 * t132 + t136) * t57 - t69 * t148;
<a name="l00228"></a>00228         jacmRT[3] = (-0.2e1 * t11 * t151 - 0.2e1 * t20 * t154 - t116 - 0.2e1 * t33 * t157 + t161) * t57 - t69 * t172;
<a name="l00229"></a>00229         jacmRT[4] = t57;
<a name="l00230"></a>00230         jacmRT[5] = 0.0e0;
<a name="l00231"></a>00231         jacmRT[6] = -t69;
<a name="l00232"></a>00232         jacmRT[7] = (-0.2e1 * t185 * t12 + t167 + (-0.2e1 * t189 * camera0 - t85) * p3D1 - 0.2e1 * t195 * t34 - t105) * t57 - t208 * t88;
<a name="l00233"></a>00233         jacmRT[8] = (-0.2e1 * t185 * t108 + t143 - 0.2e1 * t189 * t96 - 0.2e1 * t195 * t101 + t39) * t57 - t208 * t121;
<a name="l00234"></a>00234         jacmRT[9] = (-0.2e1 * t185 * t139 + t112 + (-0.2e1 * t189 * camera2 - t126) * p3D1 - 0.2e1 * t195 * t132 + t161) * t57 - t208 * t148;
<a name="l00235"></a>00235         jacmRT[10] = (-0.2e1 * t185 * t151 + t74 - 0.2e1 * t189 * t154 - 0.2e1 * t195 * t157 - t136) * t57 - t208 * t172;
<a name="l00236"></a>00236         jacmRT[11] = 0.0e0;
<a name="l00237"></a>00237         jacmRT[12] = t57;
<a name="l00238"></a>00238         jacmRT[13] = -t208;
<a name="l00239"></a>00239         jacmS[0] = t60 * t57 - t69 * t42;
<a name="l00240"></a>00240         jacmS[1] = t62 * t57 - t69 * t49;
<a name="l00241"></a>00241         jacmS[2] = t64 * t57 - t69 * t54;
<a name="l00242"></a>00242         jacmS[3] = t200 * t57 - t208 * t42;
<a name="l00243"></a>00243         jacmS[4] = t203 * t57 - t208 * t49;
<a name="l00244"></a>00244         jacmS[5] = t205 * t57 - t208 * t54;
<a name="l00245"></a>00245         }
<a name="l00246"></a>00246 
<a name="l00247"></a>00247 <span class="comment">// ---------------------------------------</span>
<a name="l00248"></a>00248 
<a name="l00249"></a>00249 <span class="keywordtype">bool</span> laSBAOptimization( <span class="keyword">const</span> QList&lt;QVCameraPose&gt; &amp;cameras,
<a name="l00250"></a>00250                                 <span class="keyword">const</span> QList&lt;QV3DPointF&gt; &amp;points3D,
<a name="l00251"></a>00251                                 <span class="keyword">const</span> QList&lt; QHash&lt;int, QPointF&gt; &gt; &amp;pointProjections,
<a name="l00252"></a>00252                                 QList&lt;QVCameraPose&gt; &amp;refinedCameras,
<a name="l00253"></a>00253                                 QList&lt;QV3DPointF&gt; &amp;refinedPoints3D,
<a name="l00254"></a>00254                                 <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> numIterations,
<a name="l00255"></a>00255                                 <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> numFixedFrames,
<a name="l00256"></a>00256                                 <span class="keyword">const</span> <span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> numFixedPoints,
<a name="l00257"></a>00257                                 <span class="keyword">const</span> <span class="keywordtype">double</span> initialMuScaleFactor,
<a name="l00258"></a>00258                                 <span class="keyword">const</span> <span class="keywordtype">double</span> stoppingThresholdForJacobian,
<a name="l00259"></a>00259                                 <span class="keyword">const</span> <span class="keywordtype">double</span> stoppingThresholdForProjections,
<a name="l00260"></a>00260                                 <span class="keyword">const</span> <span class="keywordtype">double</span> stoppingThresholdForReprojectionError,
<a name="l00261"></a>00261                                 <span class="keyword">const</span> <span class="keywordtype">double</span> stoppingThresholdForReprojectionErrorIncrement)
<a name="l00262"></a>00262         {
<a name="l00263"></a>00263         <span class="keyword">const</span> <span class="keywordtype">int</span>       numPoints3D = points3D.size(),  <span class="comment">// Number of points</span>
<a name="l00264"></a>00264                         numFrames = cameras.size(),     <span class="comment">// Frames</span>
<a name="l00265"></a>00265                         CAMERA_VECTOR_SIZE=7,           <span class="comment">// Camera size: 4 rot params + 3 trans params.</span>
<a name="l00266"></a>00266                         POINT3D_SIZE=3,                 <span class="comment">// 3D Point size.</span>
<a name="l00267"></a>00267                         POINT2D_SIZE=2;                 <span class="comment">// 2D Points size.</span>
<a name="l00268"></a>00268         
<a name="l00269"></a>00269         <span class="keywordtype">int</span> n2Dprojs = 0;
<a name="l00270"></a>00270         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; pointProjections.size(); i++)
<a name="l00271"></a>00271                 n2Dprojs += pointProjections[i].size();
<a name="l00272"></a>00272                 
<a name="l00273"></a>00273         <span class="keywordtype">double</span>  motstruct[numFrames*CAMERA_VECTOR_SIZE + numPoints3D*POINT3D_SIZE],
<a name="l00274"></a>00274                 imgpts[n2Dprojs*POINT2D_SIZE];
<a name="l00275"></a>00275 
<a name="l00276"></a>00276         <span class="keywordtype">char</span> vmask[numPoints3D * numFrames];
<a name="l00277"></a>00277         
<a name="l00278"></a>00278         <span class="comment">// -- Init data</span>
<a name="l00279"></a>00279 
<a name="l00280"></a>00280         <span class="comment">// Load camera data</span>
<a name="l00281"></a>00281         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; numFrames; i++)
<a name="l00282"></a>00282                 {
<a name="l00283"></a>00283                 <span class="keyword">const</span> <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> camera = cameras[i].operator <a class="code" href="classQVEuclideanMapping3.html" title="Class modeling rotation-translation transformations for 3D points.">QVEuclideanMapping3</a>();
<a name="l00284"></a>00284                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt; CAMERA_VECTOR_SIZE; j++)
<a name="l00285"></a>00285                         motstruct[i*CAMERA_VECTOR_SIZE+j] = camera[j];
<a name="l00286"></a>00286                 }
<a name="l00287"></a>00287 
<a name="l00288"></a>00288         <span class="comment">// Load points data</span>
<a name="l00289"></a>00289         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; numPoints3D; i++)
<a name="l00290"></a>00290                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt; POINT3D_SIZE; j++)
<a name="l00291"></a>00291                         (motstruct+numFrames*CAMERA_VECTOR_SIZE)[i*POINT3D_SIZE+j] = points3D[i][j];
<a name="l00292"></a>00292         
<a name="l00293"></a>00293         <span class="comment">// Load projections</span>
<a name="l00294"></a>00294         <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0, index = 0; j &lt; numPoints3D; j++)
<a name="l00295"></a>00295                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0; i &lt; numFrames; i++)
<a name="l00296"></a>00296                         <span class="keywordflow">if</span> (pointProjections[j].contains(i))
<a name="l00297"></a>00297                                 {
<a name="l00298"></a>00298                                 imgpts[index*POINT2D_SIZE+0] = pointProjections[j][i].x();
<a name="l00299"></a>00299                                 imgpts[index*POINT2D_SIZE+1] = pointProjections[j][i].y();
<a name="l00300"></a>00300                                 index++;
<a name="l00301"></a>00301                                 vmask[j*numFrames + i] = <span class="keyword">true</span>;
<a name="l00302"></a>00302                                 }
<a name="l00303"></a>00303                         <span class="keywordflow">else</span>    vmask[j*numFrames + i] = <span class="keyword">false</span>;
<a name="l00304"></a>00304         
<a name="l00305"></a>00305         <span class="keywordtype">double</span> opts[SBA_OPTSSZ];
<a name="l00306"></a>00306         opts[0]=initialMuScaleFactor;                           <span class="comment">// Scale factor for initial \mu,</span>
<a name="l00307"></a>00307         opts[1]=stoppingThresholdForJacobian;                   <span class="comment">// Stopping threshold for ||J^T e||_inf,</span>
<a name="l00308"></a>00308         opts[2]=stoppingThresholdForProjections;                <span class="comment">// Stopping threshold for ||dp||_2</span>
<a name="l00309"></a>00309         opts[3]=stoppingThresholdForReprojectionError;          <span class="comment">// Stopping threshold for ||e||_2 (reprojection error)</span>
<a name="l00310"></a>00310         opts[4]=stoppingThresholdForReprojectionErrorIncrement; <span class="comment">// Stopping threshold for (||e||_2-||e_new||_2)/||e||_2 (increment in the RMS reprojection error)</span>
<a name="l00311"></a>00311 
<a name="l00312"></a>00312         <span class="comment">// -- Call to sba</span>
<a name="l00313"></a>00313         <span class="keywordtype">double</span> info[SBA_INFOSZ];
<a name="l00314"></a>00314         <span class="comment">/*</span>
<a name="l00315"></a>00315 <span class="comment">                * info[0]=||e||_2 at initial p.</span>
<a name="l00316"></a>00316 <span class="comment">                * info[1-4]=[ ||e||_2, ||J^T e||_inf,  ||dp||_2, mu/max[J^T J]_ii ], all computed at estimated p.</span>
<a name="l00317"></a>00317 <span class="comment">                * info[5]= # iterations,</span>
<a name="l00318"></a>00318 <span class="comment">                * info[6]=reason for terminating: 1 - stopped by small gradient J^T e</span>
<a name="l00319"></a>00319 <span class="comment">                *                                 2 - stopped by small dp</span>
<a name="l00320"></a>00320 <span class="comment">                *                                 3 - stopped by itmax</span>
<a name="l00321"></a>00321 <span class="comment">                *                                 4 - stopped by small relative reduction in ||e||_2</span>
<a name="l00322"></a>00322 <span class="comment">                *                                 5 - stopped by small ||e||_2</span>
<a name="l00323"></a>00323 <span class="comment">                *                                 6 - too many attempts to increase damping. Restart with increased mu</span>
<a name="l00324"></a>00324 <span class="comment">                *                                 7 - stopped by invalid (i.e. NaN or Inf) &quot;func&quot; values; a user error</span>
<a name="l00325"></a>00325 <span class="comment">                * info[7]= # function evaluations</span>
<a name="l00326"></a>00326 <span class="comment">                * info[8]= # Jacobian evaluations</span>
<a name="l00327"></a>00327 <span class="comment">                * info[9]= # number of linear systems solved, i.e. number of attempts for reducing error</span>
<a name="l00328"></a>00328 <span class="comment">        */</span>
<a name="l00329"></a>00329 
<a name="l00330"></a>00330         QTime time;
<a name="l00331"></a>00331         time.start();
<a name="l00332"></a>00332         sba_iterations = sba_motstr_levmar(     numPoints3D,            <span class="comment">// Number of 3D points.</span>
<a name="l00333"></a>00333                                                 numFixedPoints,         <span class="comment">// Number of fixed points.</span>
<a name="l00334"></a>00334                                                 numFrames,              <span class="comment">// Number of images/cameras.</span>
<a name="l00335"></a>00335                                                 numFixedFrames,         <span class="comment">// Number of constant images/cameras.</span>
<a name="l00336"></a>00336                                                 vmask,                  <span class="comment">// Point visibility for each frame.</span>
<a name="l00337"></a>00337                                                 motstruct,              <span class="comment">// Cameras + points.</span>
<a name="l00338"></a>00338                                                 CAMERA_VECTOR_SIZE,     <span class="comment">// Camera size.</span>
<a name="l00339"></a>00339                                                 POINT3D_SIZE,           <span class="comment">// 3D Point size.</span>
<a name="l00340"></a>00340                                                 imgpts,                 <span class="comment">// Pointer to image projections.</span>
<a name="l00341"></a>00341                                                 NULL,                   <span class="comment">// Variance matrices for the image projections.</span>
<a name="l00342"></a>00342                                                 POINT2D_SIZE,           <span class="comment">// Size of each projected point.</span>
<a name="l00343"></a>00343                                                 my_img_proj,            <span class="comment">// Projection function.</span>
<a name="l00344"></a>00344                                                 my_img_proj_jac,        <span class="comment">// Jacobian function of the projection function.</span>
<a name="l00345"></a>00345                                                 NULL,                   <span class="comment">// *data.</span>
<a name="l00346"></a>00346                                                 numIterations,          <span class="comment">// Max iterations.</span>
<a name="l00347"></a>00347                                                 <span class="keyword">false</span>,                  <span class="comment">// Print debug messages.</span>
<a name="l00348"></a>00348                                                 opts,                   <span class="comment">// Options</span>
<a name="l00349"></a>00349                                                 info                    <span class="comment">// Output info.</span>
<a name="l00350"></a>00350                                                 );
<a name="l00351"></a>00351         sba_elapsed_milisecs = time.elapsed();  
<a name="l00352"></a>00352         refinedPoints3D = points3D;
<a name="l00353"></a>00353 
<a name="l00354"></a>00354         <span class="comment">// -- Get data from sba structures</span>
<a name="l00355"></a>00355         <span class="comment">// Load camera data</span>
<a name="l00356"></a>00356         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; numFrames; i++)
<a name="l00357"></a>00357                 {
<a name="l00358"></a>00358                 <a class="code" href="classQVVector.html" title="Implementation of numerical vectors.">QVVector</a> cameraVector(CAMERA_VECTOR_SIZE);
<a name="l00359"></a>00359                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt; CAMERA_VECTOR_SIZE; j++)
<a name="l00360"></a>00360                         cameraVector[j] = motstruct[i*CAMERA_VECTOR_SIZE+j];
<a name="l00361"></a>00361                 refinedCameras &lt;&lt; <a class="code" href="classQVEuclideanMapping3.html" title="Class modeling rotation-translation transformations for 3D points.">QVEuclideanMapping3</a>(cameraVector);
<a name="l00362"></a>00362                 }
<a name="l00363"></a>00363 
<a name="l00364"></a>00364         <span class="comment">// Load points data</span>
<a name="l00365"></a>00365         <span class="keywordflow">for</span> (<span class="keywordtype">int</span> i = 0; i &lt; numPoints3D; i++)
<a name="l00366"></a>00366                 <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 0; j &lt; POINT3D_SIZE; j++)
<a name="l00367"></a>00367                         refinedPoints3D[i][j] = (motstruct+numFrames*CAMERA_VECTOR_SIZE)[i*POINT3D_SIZE+j];
<a name="l00368"></a>00368 
<a name="l00369"></a>00369         sba_stop_condition = info[6];
<a name="l00370"></a>00370         initial_reprojection_error = info[0];
<a name="l00371"></a>00371         final_reprojection_error = info[1];
<a name="l00372"></a>00372 
<a name="l00373"></a>00373         <span class="keywordflow">return</span> (info[6] &gt; 0 and info[6] &lt; 6);
<a name="l00374"></a>00374         }
<a name="l00375"></a>00375 
<a name="l00376"></a>00376 
</pre></div></div>
</td></tr></table>

<br /><hr><br />
<center><a href="http://perception.inf.um.es/QVision">QVision framework</a>.
<a href="http://perception.inf.um.es">PARP research group</a>.
Copyright &copy; 2007, 2008, 2009, 2010, 2011.</center>
<br />
</body>
</html>
